package ar.edu.untref.robotica.simulador;

import java.awt.Point;
import java.awt.Rectangle;
import java.io.IOException;
import java.math.BigDecimal;
import java.util.Iterator;
import java.util.List;
import java.util.Timer;
import java.util.TimerTask;

import javax.swing.JSpinner;
import javax.swing.event.ChangeEvent;
import javax.swing.event.ChangeListener;

import navegacion.QuadTreeT;
import ar.edu.untref.robotica.control.ConexionRemota;
import ar.edu.untref.robotica.control.ModuloDeControl;
import ar.edu.untref.robotica.control.Teclado;
import ar.edu.untref.robotica.log.Log;
import ar.edu.untref.robotica.motorfisico.Calculadora;

public class Simulador extends TimerTask {

	private int fps = 120;
	private Graficador graficador;
	private Escenario escenario;
	private Robot robot;
	private double velocidadFinalDeseadaIzquierda = 0;
	private double velocidadFinalDeseadaDerecha = 0;
	private double fuerzaAceleracion = 0.05;
	private Posicion posicionAnterior;
	private ModuloDeControl controlador;
	private ModuloDeControl teclado;
	private Log logger;
	private int contadorLog = 0;
	
	private DetectorDeChoque detectorArribaI = new DetectorDeChoque(-15, -15); // Calculable
	private DetectorDeChoque detectorArribaD = new DetectorDeChoque(+15, -15); // Calculable
	private DetectorDeChoque detectorAbajoI = new DetectorDeChoque(-15, +15); // Calculable
	private DetectorDeChoque detectorAbajoD = new DetectorDeChoque(+15, +15); // Calculable

	public Simulador(Escenario escenario) {

		this.escenario = escenario;
	}

	public void inicializar() {

		this.graficador = new Graficador(this.escenario);
		
		this.robot = new Robot(Configuracion.instancia().getAltoRobot(),
				Configuracion.instancia().getAnchoRobot());
		Timer timer = new Timer();
		double period = (double) 1 / this.fps * 1000;
		timer.scheduleAtFixedRate(this, 0, (long) period);
		
		this.logger = new Log();
		try {
			this.logger.crearFile();
		} catch (IOException e) {

			e.printStackTrace();
		}
		
		///////quad y pruebas///////
		
		QuadTreeT quad = new QuadTreeT(escenario.getMatriz(), 0, new Rectangle (0,0,escenario.getAncho(),escenario.getAlto()));
		
		QuadTreeT t = quad.buscarNodo(159, 220);
		System.out.println("x: "+t.getBounds().getX()+" "+(t.getBounds().getX()+t.getBounds().getWidth()-1));
		System.out.println("y: "+t.getBounds().getY()+" "+(t.getBounds().getY()+t.getBounds().getHeight()-1));
		System.out.println("hay obstaculo: " +t.getEstado());
		
		//recorrido
		System.out.println("\n\n\n");
		System.out.println("Recorrido: \n");
		
		QuadTreeT nodoInicio = quad.buscarNodo(escenario.getPosicionRobot().getPosicionX(), escenario.getPosicionRobot().getPosicionY());
		List<QuadTreeT> recorrido = quad.getRutaADestino(nodoInicio, 325, 250, escenario.getAlto(), escenario.getAncho());
		Iterator<QuadTreeT> itRecorrido = recorrido.iterator();

		while(itRecorrido.hasNext()){
			t= itRecorrido.next();
			System.out.println("x: "+t.getBounds().getX()+" "+(t.getBounds().getX()+t.getBounds().getWidth()-1));
			System.out.println("y: "+t.getBounds().getY()+" "+(t.getBounds().getY()+t.getBounds().getHeight()-1));
			System.out.println("\n");
		}

		////////Fin quad y pruebas///////
		
		this.teclado = new Teclado(this);
		this.controlador = new ConexionRemota(this);
		
	}

	@Override
	public void run() {

		this.contadorLog++;
		if (Calculadora.getVelocidadDesplazamiento(
				this.velocidadFinalDeseadaIzquierda,
				this.velocidadFinalDeseadaDerecha) > 0) {
			if (!puedeAvanzar()) {
				acelerarRobot(0, 0);
			}
		} else if (Calculadora.getVelocidadDesplazamiento(
				this.velocidadFinalDeseadaIzquierda,
				this.velocidadFinalDeseadaDerecha) < 0) {
			if (!puedeRetroceder()) {
				acelerarRobot(0, 0);
			}
		}
		this.logicaAceleracion();
		this.escenario.aumentarAngulo(Calculadora.getCambioAngulo(
				this.robot.getRuedaI(), this.robot.getRuedaD())
				/ this.fps);
		this.reflejarMovimiento();
		this.graficador.sensar(this.robot);
		
		if (this.contadorLog == 60){
			
			try {
				
				this.logger.guardarDatos(graficador.datosRelevantesAGuardar());
				
			} catch (IOException e) {
			
				e.printStackTrace();
			}
			
			this.contadorLog = 0;
		}
		
		this.menuComandosRobot();
		if (this.hayChoque()) {
			robot.getRuedaD().setVelocidad(new BigDecimal(0));
			robot.getRuedaI().setVelocidad(new BigDecimal(0));
			this.acelerarRobot(0, 0);
			this.escenario.setPosicionRobot(posicionAnterior);
		} else {
			this.posicionAnterior = this.escenario.getPosicionRobot().clone();
		}
	}

	public boolean puedeAvanzar() {

		double distanciaSegura = Configuracion.instancia().getDistanciaSegura();
		return this.robot.getDistanciaObstaculoAdelante(this.escenario) > distanciaSegura
				&& this.robot.getDistanciaObstaculo45Derecha(this.escenario) > distanciaSegura
				&& this.robot.getDistanciaObstaculo45Izquierda(this.escenario) > distanciaSegura;
	}

	public boolean puedeRetroceder() {

		double distanciaSegura = Configuracion.instancia().getDistanciaSegura();
		return this.robot.getDistanciaObstaculoAtras(this.escenario) > distanciaSegura;
	}

	public void reflejarMovimiento() {

		Posicion posicionRobot = this.escenario.getPosicionRobot();
		double posicionXActual = posicionRobot.getPosicionX();
		double posicionYActual = posicionRobot.getPosicionY();
		double velocidad = this.robot.getVelocidadDesplazamiento();
		double angulo = Math.toRadians(this.escenario.getPosicionRobot()
				.getAngulo());
		double posicionXNueva = posicionXActual + velocidad * Math.sin(angulo);
		double posicionYNueva = posicionYActual - velocidad * Math.cos(angulo);
		posicionRobot.setPosicionX(posicionXNueva);
		posicionRobot.setPosicionY(posicionYNueva);
		this.graficador.repaint();
	}

	private void logicaAceleracion() {

		BigDecimal deltaDeAceleracion = new BigDecimal(this.fuerzaAceleracion);
		if (!velocidadDeseadaAlcanzadaIzquierda()) {
			if (this.velocidadFinalDeseadaIzquierda > this.robot.getRuedaI()
					.getVelocidad().doubleValue()) {
				this.robot.getRuedaI().setVelocidad(
						this.robot.getRuedaI().getVelocidad()
								.add(deltaDeAceleracion));
			} else if (this.velocidadFinalDeseadaIzquierda < this.robot
					.getRuedaI().getVelocidad().doubleValue()) {
				this.robot.getRuedaI().setVelocidad(
						this.robot.getRuedaI().getVelocidad()
								.subtract(deltaDeAceleracion));
			}
		}
		if (!velocidadDeseadaAlcanzadaDerecha()) {
			if (this.velocidadFinalDeseadaDerecha > this.robot.getRuedaD()
					.getVelocidad().doubleValue()) {
				this.robot.getRuedaD().setVelocidad(
						this.robot.getRuedaD().getVelocidad()
								.add(deltaDeAceleracion));
			} else if (this.velocidadFinalDeseadaDerecha < this.robot
					.getRuedaD().getVelocidad().doubleValue()) {
				this.robot.getRuedaD().setVelocidad(
						this.robot.getRuedaD().getVelocidad()
								.subtract(deltaDeAceleracion));
			}
		}
	}

	public void acelerarRobot(double velocidadFinalIzquierda,
			double velocidadFinalDerecha) {

		this.velocidadFinalDeseadaIzquierda = velocidadFinalIzquierda;
		this.velocidadFinalDeseadaDerecha = velocidadFinalDerecha;
	}

	private boolean velocidadDeseadaAlcanzadaIzquierda() {

		return this.robot.getRuedaI().getVelocidad().doubleValue() == this.velocidadFinalDeseadaIzquierda;
	}

	private boolean velocidadDeseadaAlcanzadaDerecha() {

		return this.robot.getRuedaD().getVelocidad().doubleValue() == this.velocidadFinalDeseadaDerecha;
	}

	private void menuComandosRobot() {

		final Integer valorRuedaDerecha = (Integer) this.graficador
				.getSpinnerD().getValue();
		final Integer valorRuedaIzquierda = (Integer) this.graficador
				.getSpinnerI().getValue();
		this.graficador.getSpinnerD().addChangeListener(new ChangeListener() {

			@Override
			public void stateChanged(ChangeEvent e) {

				Integer valorAntRD = (Integer) ((JSpinner) e.getSource())
						.getPreviousValue();
				if (valorRuedaDerecha > valorAntRD) {
					acelerarRobot(
							Simulador.this.velocidadFinalDeseadaIzquierda,
							Simulador.this.velocidadFinalDeseadaDerecha - 1);
				} else {
					acelerarRobot(
							Simulador.this.velocidadFinalDeseadaIzquierda,
							Simulador.this.velocidadFinalDeseadaDerecha + 1);
				}
				Simulador.this.graficador.getBarraDer().setValue(
						(int) Simulador.this.graficador.getSpinnerD()
								.getValue());
			}
		});
		this.graficador.getSpinnerI().addChangeListener(new ChangeListener() {

			@Override
			public void stateChanged(ChangeEvent e) {

				Integer valorAntRI = (Integer) ((JSpinner) e.getSource())
						.getPreviousValue();
				if (valorRuedaIzquierda > valorAntRI) {
					acelerarRobot(
							Simulador.this.velocidadFinalDeseadaIzquierda - 1,
							Simulador.this.velocidadFinalDeseadaDerecha);
				} else {
					acelerarRobot(
							Simulador.this.velocidadFinalDeseadaIzquierda + 1,
							Simulador.this.velocidadFinalDeseadaDerecha);
				}
				Simulador.this.graficador.getBarraIzq().setValue(
						(int) Simulador.this.graficador.getSpinnerI()
								.getValue());
			}
		});
	}

	private boolean hayChoque() {

		boolean choco = false;
		int x = this.escenario.getShapeRobot().getBounds().x;
		int y = this.escenario.getShapeRobot().getBounds().y;
		int ancho = this.escenario.getShapeRobot().getBounds().width;
		int alto = this.escenario.getShapeRobot().getBounds().height;
		for (int i = x; i < x + ancho && i < this.escenario.getMatriz().length
				&& !choco; i++) {
			for (int j = y; j < y + alto
					&& j < this.escenario.getMatriz()[i].length && !choco; j++) {
				if (this.escenario.getMatriz()[i][j] == 1) {
					Point cadaPunto = new Point(i, j);
					if (this.escenario.getShapeRobot().contains(cadaPunto)) {
						choco = true;
					}
				}
			}
		}
		return choco;
	}
	
	public double getVelocidadFinalDeseadaIzquierda(){
		
		return velocidadFinalDeseadaIzquierda;
	}
	
	public double getVelocidadFinalDeseadaDerecha(){
		
		return velocidadFinalDeseadaDerecha;
	}

	public Escenario getEscenario() {
		return escenario;
	}

	public Robot getRobot() {
		return robot;
	}
	
	public Graficador getGraficador(){
		return graficador;
	}
	
}
